#! /usr/bin/env python3
import rospy
from nav_msgs.msg import Odometry
import json
import sys

car_info_dict = {}
start_time = 0
last_time = 0
car_num = 70

def odom_callback(msg, car_id):
    global start_time, last_time
    cur_time = rospy.get_time()
    if round((cur_time - last_time), 1) < 0.1:
      return
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    z = msg.pose.pose.position.z
    yaw = msg.pose.pose.orientation.z
    t = cur_time - start_time
    last_time = cur_time
    
    if car_id not in car_info_dict:
        car_info_dict[car_id] = []
    car_info_dict[car_id].append({'x': round(x, 2), 'y': round(y, 2), 'z': round(z, 2), 'yaw': round(yaw, 2), 't': round(t, 2)})
    print(f'current time: {t}')

def save_car_info():
    # 将字典保存为JSON文件
    with open(f'car{car_num}_info.json', 'w') as file:
        json.dump(car_info_dict, file)

if __name__ == '__main__':
    rospy.init_node('car_info_subscriber')
    start_time = rospy.get_time()
    last_time = rospy.get_time()
    car_num = int(sys.argv[1])
    car_ids = ['car_{}'.format(i) for i in range(car_num)]
    car_topics = ['/car_{}/odometry'.format(i) for i in range(car_num)]

    subscribers = []
    for car_topic in car_topics:
        sub = rospy.Subscriber(car_topic, Odometry, odom_callback, callback_args=car_topic)
        subscribers.append(sub)

    rate = rospy.Rate(10)  # 设置订阅频率为10Hz

    while not rospy.is_shutdown():
        # 每0.1s打印并保存一次小车信息
        # print(car_info_dict)
        save_car_info()
        rate.sleep()